﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Maubot.Motion;
using Maubot.Modeling;
using Maubot.MathFunctions;
using Robocode;

namespace Maubot.Motion
{
    public class MotionModule
    {
        public Pose CurrentPose { get; set; }
        public Position TargetPosition { get; set; }
        public MoveCommand MoveCommand { get; set; }

        public MotionModule(Pose currentPose = null, Position targetPosition = null, MoveCommand command = null)
        {
            CurrentPose = currentPose ?? new Pose();
            TargetPosition = targetPosition ?? new Position();
            MoveCommand = command ?? new MoveCommand();
        }

        public void Execute()
        {
            double distance = Calcolator.PointsDistance(CurrentPose, TargetPosition);
            double deltaAngle = Calcolator.DeltaAngle(CurrentPose, TargetPosition);
            double minAngle = Calcolator.DeltaInvariantAngle(CurrentPose, TargetPosition);
            double gain = SpeedGain(deltaAngle);
            
            MoveCommand.Speed = gain * distance;
            MoveCommand.Angle = minAngle;
        }

        public double SpeedGain(double angle)
        {
            return 1 - Math.Abs(angle / 90);
        }
    }
}
